//
// Created by 86137 on 2024/1/25.
//

#include "can_communicate_task.h"

CtrlMsg_arm ctrl_arm_msg={};
CtrlMsg_arm init_ctrl_arm_msg={370,0,50};
CtrlMsg_vision ctrl_vision_msg={};
ctrl_msg_tran my_tx;
uint8_t is_ready_to_tx=0;
uint8_t first_rx_init=0;
extern ctrl_msg_data my_msg;
extern rc_device_t g_rc;
void rx_arm_callback(can_device_receive *can_receive, uint8_t *data){
    ctrl_arm_msg_receive * ctrl_msg_t = (ctrl_arm_msg_receive *) data;
    ctrl_arm_msg.is_ready=ctrl_msg_t->is_ready;
    if(ctrl_arm_msg.is_ready==1) {
        ctrl_arm_msg.x = ctrl_msg_t->x;
        ctrl_arm_msg.y = ctrl_msg_t->y;
        ctrl_arm_msg.z = ctrl_msg_t->z;
        ctrl_arm_msg.yaw = ctrl_msg_t->yaw;
        ctrl_arm_msg.x = ctrl_arm_msg.x * GAIN_RX;
        ctrl_arm_msg.y = ctrl_arm_msg.y * GAIN_RX;
        ctrl_arm_msg.z = ctrl_arm_msg.z * GAIN_RX;
        ctrl_arm_msg.yaw = ctrl_arm_msg.yaw * GAIN_RX;
    }
//    if(ctrl_arm_msg.is_ready==1 && first_rx_init==0) {
//        init_ctrl_arm_msg.x = ctrl_msg_t->x;
//        init_ctrl_arm_msg.y = ctrl_msg_t->y;
//        init_ctrl_arm_msg.z = ctrl_msg_t->z;
//        init_ctrl_arm_msg.yaw = ctrl_msg_t->yaw;
//        init_ctrl_arm_msg.x = ctrl_arm_msg.x * GAIN_RX;
//        init_ctrl_arm_msg.y = ctrl_arm_msg.y * GAIN_RX;
//        init_ctrl_arm_msg.z = ctrl_arm_msg.z * GAIN_RX;
//        init_ctrl_arm_msg.yaw = ctrl_arm_msg.yaw * GAIN_RX;
//        first_rx_init=1;
//    }
//    if(ctrl_arm_msg.is_ready==0){
//        ctrl_arm_msg.x = 0;
//        ctrl_arm_msg.y = 0;
//        ctrl_arm_msg.z = 0;
//        ctrl_arm_msg.yaw = 0;
//    }
}
void rx_vision_0_callback(can_device_receive *can_receive, uint8_t *data){
    auto ctrl_msg_t_1 = (ctrl_vision_msg_0_receive *) data;
    ctrl_vision_msg.x = ctrl_msg_t_1->x;
    ctrl_vision_msg.y = ctrl_msg_t_1->y;

    ctrl_vision_msg.x = ctrl_vision_msg.x ;
    ctrl_vision_msg.y = ctrl_vision_msg.y ;
}
void rx_vision_z_callback(can_device_receive *can_receive, uint8_t *data){
    auto ctrl_msg_t_1 = (ctrl_vision_msg_z_receive *) data;
    ctrl_vision_msg.z = ctrl_msg_t_1->z;
    ctrl_vision_msg.target_detected = ctrl_msg_t_1->target_detected;

    ctrl_vision_msg.z = ctrl_vision_msg.z ;
    ctrl_vision_msg.target_detected = ctrl_vision_msg.target_detected ;
}

void can_communicate_task(void *argument){

    can_device_transmit can_com_tx(&hcan2,0x301,8,(uint8_t *)&my_tx);
    can_device_receive can_com_arm_rx(&hcan2,0x300,rx_arm_callback);
    can_device_receive can_com_vision_0_rx(&hcan2,0x302,rx_vision_0_callback);
    can_device_receive can_com_vision_z_rx(&hcan2,0x303,rx_vision_z_callback);
    for(;;)
    {
//        if(is_ready_to_tx==0){
//            my_tx.x=(int16_t)(init_ctrl_arm_msg.x*GAIN_TX);
//            my_tx.y=(int16_t)(init_ctrl_arm_msg.y*GAIN_TX);
//            my_tx.z=(uint16_t)(init_ctrl_arm_msg.z*GAIN_TX);
//            my_tx.yaw=(int16_t)(init_ctrl_arm_msg.yaw*GAIN_TX);
//            my_tx.is_cut_on=my_msg.is_cut_on;
//
//            if(first_rx_init==1) {
//                my_tx.msg_able = 1;
//                is_ready_to_tx = 1;
//            }
//            else if(first_rx_init==0) my_tx.msg_able=1;

        //}
//        else if(is_ready_to_tx==1){
            send_calculate();
            my_tx.x = (int16_t) (my_msg.x * GAIN_TX);
            my_tx.y = (int16_t) (my_msg.y * GAIN_TX);
            my_tx.z = (uint16_t) (my_msg.z * GAIN_TX);
            my_tx.yaw = (int16_t) (my_msg.yaw * GAIN_TX);
            my_tx.is_cut_on = my_msg.is_cut_on;
            my_tx.msg_able = 1;
  //      }
        if(g_rc.is_lost==0 && g_rc.rc_info.sw.sw_l==RC_SW_MID)
        can_com_tx.can_send_msg();
        osDelay(2);
    }
}